#
# Copyright 2005,2006 Free Software Foundation, Inc.
# 
# This file is part of GNU Radio
# 
# GNU Radio is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2, or (at your option)
# any later version.
# 
# GNU Radio is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
# 
# You should have received a copy of the GNU General Public License
# along with GNU Radio; see the file COPYING.  If not, write to
# the Free Software Foundation, Inc., 51 Franklin Street,
# Boston, MA 02110-1301, USA.
# 

from gnuradio import gr, gru, xstar
from math import pi
import gnuradio.gr.gr_threading as _threading
import Numeric
import struct
import sys
import time

HEADER_TYPE = ord('0')
VOICE_TYPE = ord('1')
SYNC_TYPE = ord('3')
END_TYPE = 255

def conv_1_0_string_to_packed_binary_string(s):
    """
    '10101111' -> ('\xAF', False)

    Basically the inverse of conv_packed_binary_string_to_1_0_string,
    but also returns a flag indicating if we had to pad with leading zeros
    to get to a multiple of 8.
    """
    if not is_1_0_string(s):
        raise ValueError, "Input must be a string containing only 0's and 1's"
    
    # pad to multiple of 8
    padded = False
    rem = len(s) % 8
    if rem != 0:
        npad = 8 - rem
        s = '0' * npad + s
        padded = True

    assert len(s) % 8 == 0

    r = []
    i = 0
    while i < len(s):
        t = 0
        for j in range(8):
            t = (t << 1) | (ord(s[i + j]) - ord('0'))
        r.append(chr(t))
        i += 8
    return (''.join(r), padded)

xstar_access_code = '111011001010000'
preamble = '1010' * 16
postamble = '1010' * 8 + '000100110101111' + '0'

def is_1_0_string(s):
    if not isinstance(s, str):
        return False
    for ch in s:
        if not ch in ('0', '1'):
            return False
    return True

# /////////////////////////////////////////////////////////////////////////////
#                   mod/demod with packets as i/o
# /////////////////////////////////////////////////////////////////////////////

access_code = '111011001010000'
preamble = '1010' * 16
postamble = '1010' * 8 + '000100110101111' + '0'

sync_code = '001011001010100110001011'

class mod_pkts(gr.hier_block):
    """
    Wrap an arbitrary digital modulator in our packet handling framework.

    Send packets by calling send_pkt
    """
    def __init__(self, fg, modulator, access_code=None, msgq_limit=2, pad_for_usrp=True):
        """
	Hierarchical block for sending packets

        Packets to be sent are enqueued by calling send_pkt.
        The output is the complex modulated signal at baseband.

	@param fg: flow graph
	@type fg: flow graph
        @param modulator: instance of modulator class (gr_block or hier_block)
        @type modulator: complex baseband out
        @param access_code: AKA sync vector
        @type access_code: string of 1's and 0's between 1 and 64 long
        @param msgq_limit: maximum number of messages in message queue
        @type msgq_limit: int
        @param pad_for_usrp: If true, packets are padded such that they end up a multiple of 128 samples

        See gmsk_mod for remaining parameters
        """
        self._modulator = modulator
        self._pad_for_usrp = pad_for_usrp
    	self._fec = xstar.reed_solomon(63, 21) # TODO: Move?

        if access_code is None:
            access_code = xstar_access_code
        if not is_1_0_string(access_code):
            raise ValueError, "Invalid access_code %r. Must be string of 1's and 0's" % (access_code,)
        self._access_code = access_code

        # accepts messages from the outside world
        self._pkt_input = gr.message_source(gr.sizeof_char, msgq_limit)
        fg.connect(self._pkt_input, self._modulator)
        gr.hier_block.__init__(self, fg, None, self._modulator)

        self.sending = False

        self.frames_since_sync = 0

    def underflow(self):
        if self.sending:
            self.send_sync()

    def send_sync(self):
        (packed_sync_code, ignore) = conv_1_0_string_to_packed_binary_string(sync_code)
        self.send_pkt(packed_sync_code, type=SYNC_TYPE);

    def empty_queue(self):
        return self._pkt_input.msgq().empty_p()

    def stop_frame(self):
        self.sending = False
        self._underflow_watcher.keep_running = False

        # Send end of voice packet
        pkt = chr(END_TYPE) * 64

        msg = gr.message_from_string(pkt)
        self._pkt_input.msgq().insert_tail(msg)

    def send_pkt(self, payload, type=VOICE_TYPE):
        """
        Send the payload.

        @param payload: data to send
        @type payload: string
        """

        pkt = ""

        if type == SYNC_TYPE:
            self.frames_since_sync = 0
        else:
            self.frames_since_sync += 1

            if self.frames_since_sync > 10:
                self.send_sync()

        if self.sending == False:
            import time
            time.sleep(0.015)
            self._underflow_watcher = _underflow_watcher_thread(self)
            self.sending = True
            self.voice_frames_sent = 0

            # Send preamble and voice packet header
            (packed_access_code, padded) = conv_1_0_string_to_packed_binary_string(access_code)
            (packed_preamble, ignore) = conv_1_0_string_to_packed_binary_string(preamble)

            header = '{HEADER: (ABCDEFGHIKLMNOPQRSTUVXYZabcde)}' 
            pkt = ''.join((packed_preamble, packed_access_code, header))

        if type == VOICE_TYPE:
            # Add Reed-Solomon FEC for outgoing voice frames
            assert len(payload) == 63 - 21
    	    payload = payload + self._fec.encode(payload)

            if True: #self._simulate_errors:
	        from random import random
	        payload = [x for x in payload]

	        for x in range(10):
		    payload[int(random()*41)] = 'x'

	        payload = "".join(payload)


        pkt += chr(type)
        pkt += payload
    
        if type == VOICE_TYPE:
	    # "Scrambled" padding up to 64 bytes
            pkt += ("0123456789"*7)[0:(63 - len(payload))]

        import time
#        print >> sys.stderr, "Out: %5.2f %3d %s" % (time.time(), len(pkt), repr(pkt))

        if type == VOICE_TYPE:
            self.voice_frames_sent += 1

        msg = gr.message_from_string(pkt)
        self._pkt_input.msgq().insert_tail(msg)


class demod_pkts(gr.hier_block):
    """
    Wrap an arbitrary digital demodulator in our packet handling framework.

    The input is complex baseband.  When packets are demodulated, they are passed to the
    app via the callback.
    """

    def __init__(self, fg, demodulator, access_code=None, callback=None, threshold=-1):
        """
	Hierarchical block for demodulating and deframing packets.

	The input is the complex modulated signal at baseband.
        Demodulated packets are sent to the handler.

	@param fg: flow graph
	@type fg: flow graph
        @param demodulator: instance of demodulator class (gr_block or hier_block)
        @type demodulator: complex baseband in
        @param access_code: AKA sync vector
        @type access_code: string of 1's and 0's
        @param callback:  function of two args: ok, payload
        @type callback: ok: bool; payload: string
        @param threshold: detect access_code with up to threshold bits wrong (-1 -> use default)
        @type threshold: int
	"""

        self._demodulator = demodulator
        if access_code is None:
            access_code = xstar_access_code
        if not is_1_0_string(access_code):
            raise ValueError, "Invalid access_code %r. Must be string of 1's and 0's" % (access_code,)
        self._access_code = access_code

        if threshold == -1:
            threshold = 12              # FIXME raise exception

        self._rcvd_pktq = gr.msg_queue()          # holds packets from the PHY
        self.correlator = gr.correlate_access_code_bb(access_code, threshold)

        self.framer_sink = xstar.framer_sink(self._rcvd_pktq)
        self.framer_sink.set_postamble('1010' * 8 + '000100110101111' + '0')
        fg.connect(self._demodulator, self.correlator, self.framer_sink)
        
        gr.hier_block.__init__(self, fg, self._demodulator, None)
        self._watcher = _queue_watcher_thread(self._rcvd_pktq, callback)

class _underflow_watcher_thread(_threading.Thread):
    def __init__(self, modulator):
        _threading.Thread.__init__(self)
        self.setDaemon(1)
        self.modulator = modulator
        self.keep_running = True
        self.start()

    def run(self):
        while self.keep_running:
            time.sleep(0.001)
            if self.modulator.empty_queue():
                self.modulator.underflow()

class _queue_watcher_thread(_threading.Thread):
    def __init__(self, rcvd_pktq, callback):
        _threading.Thread.__init__(self)
        self.setDaemon(1)
        self.rcvd_pktq = rcvd_pktq
        self.callback = callback
        self.keep_running = True
        self.start()
	self._fec = xstar.reed_solomon(63, 21) # TODO


    def run(self):
        while self.keep_running:
            msg = self.rcvd_pktq.delete_head()
            #print >>sys.stderr, "_queue_watcher_thread:", msg.type(), repr(msg.to_string())
            ok = True

            payload = msg.to_string()
	    if msg.type() == VOICE_TYPE:
		decoded = self._fec.decode(payload)
		if len(decoded) != 0:
                    payload = decoded
		else:
                    ok = False

            if self.callback:
                self.callback(ok, payload, msg.type())

# vim: sts=4 sw=4 et
